#include "../include/chassis_driver/chassis_driver.h"
// #include <algorithmfwd.h>
#include <complex>
#include <deque>

// 构造函数
ChassisDriver::ChassisDriver()
    : Node("chassis_driver")
    , send_buffer_{FRAME_HEADER,0,0,0,0,0,0,0,0,FRAME_TAIL}
    , receive_buffer_{0}
    , socket_initialized_(false)
    , is_connected_(false)
    , udp_socket_(INVALID_SOCKET)
    , receive_timeout_ms_(1000)
    , sampling_time_(0.0)
    , battery_voltage_(0.0)
    , last_update_time_(this->get_clock()->now())
    , robot_pose_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0})
    , robot_velocity_({0.0, 0.0, 0.0})
    , vx_history_()
    , vy_history_()
    , vtheta_history_()
    , last_vx_(0.0)
    , last_vy_(0.0)
    , last_vw_(0.0)
{
    initializeParameters();
    initializePublishers();
    initializeSubscribers();
    
    if (!initSocket()) {
        RCLCPP_ERROR(get_logger(), "Failed to initialize UDP communication");
        return;
    }
    memset(send_buffer_, 0, SEND_DATA_SIZE);
    send_buffer_[0] = FRAME_HEADER;
    send_buffer_[8] = calculateChecksum(send_buffer_, 8);
    send_buffer_[9] = FRAME_TAIL;
    sendData(send_buffer_, SEND_DATA_SIZE);
    socket_initialized_ = true;
    RCLCPP_INFO(get_logger(), "UDP broadcast initialized on port %d", local_port_);

    // 初始化时间戳为当前时间
    last_update_time_ = this->get_clock()->now();  // 使用节点时钟
}

// 析构函数
ChassisDriver::~ChassisDriver()
{
    if (socket_initialized_) {
        // 发送停止命令
        memset(send_buffer_, 0, SEND_DATA_SIZE);
        send_buffer_[0] = FRAME_HEADER;
        send_buffer_[8] = calculateChecksum(send_buffer_, 8);
        send_buffer_[9] = FRAME_TAIL;
        sendData(send_buffer_, SEND_DATA_SIZE);
        
        closeSocket();
    }
    RCLCPP_INFO(get_logger(), "Shutting down chassis driver");
}

// 初始化参数
void ChassisDriver::initializeParameters()
{
    // 声明并获取参数
    this->declare_parameter<std::string>("remote_ip", "192.168.19.80");
    this->declare_parameter<int>("remote_port", 16888);
    this->declare_parameter<int>("local_port", 16888);
    this->declare_parameter<std::string>("odom_frame_id", "odom");
    this->declare_parameter<std::string>("robot_frame_id", "base_link");
    this->declare_parameter<std::string>("cmd_vel_topic", "cmd_vel");
    
    this->get_parameter("remote_ip", remote_ip_);
    this->get_parameter("remote_port", remote_port_);
    this->get_parameter("local_port", local_port_);
    this->get_parameter("odom_frame_id", odom_frame_id_);
    this->get_parameter("robot_frame_id", robot_frame_id_);
    this->get_parameter("cmd_vel_topic", cmd_vel_topic_);
}

// 初始化发布者
void ChassisDriver::initializePublishers()
{
    odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    voltage_publisher_ = this->create_publisher<std_msgs::msg::Float32>("voltage", 10);
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
}

// 初始化订阅者
void ChassisDriver::initializeSubscribers()
{
    cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
        cmd_vel_topic_, 10,
        std::bind(&ChassisDriver::cmdVelCallback, this, std::placeholders::_1));
}

// 主循环函数
void ChassisDriver::runLoop()
{
    rclcpp::Time current_time = now();
    rclcpp::Time last_time = current_time;
    
    while (rclcpp::ok())
    {
        // 处理ROS消息
        rclcpp::spin_some(get_node_base_interface());
        if (waitData())
        {
            updateRobotPose();
            publishRobotState();
        }
    }
}

// 更新机器人位姿
void ChassisDriver::updateRobotPose() {
    // 1. 获取当前时间并计算时间间隔
    auto current_time = this->get_clock()->now();  // 使用节点时钟
    double dt = (current_time - last_update_time_).seconds();
    
    // 2. 获取机器人速度
    double vx = robot_velocity_.linear_x;
    double vy = robot_velocity_.linear_y;
    double vtheta = robot_velocity_.angular_z;  // 使用angular_z而不是linear_z
    // double vx = calculateMovingAverage(vx_history_, robot_velocity_.linear_x);
    // double vy = calculateMovingAverage(vy_history_, robot_velocity_.linear_y);
    // double vtheta = calculateMovingAverage(vtheta_history_, robot_velocity_.angular_z);
    
    // 3. 计算位置变化量（考虑轮距的差速轮运动学）
    double delta_x = vx * cos(robot_pose_.yaw) * dt;
    double delta_y = vx * sin(robot_pose_.yaw) * dt;
    double delta_theta = vtheta * dt;
    // updateVelocityHistory(vx, vy, vtheta);
    
    // // 4. 计算位置变化量（使用精确的运动学模型）
    // double delta_x, delta_y, delta_theta;
    // calculateKinematics(vx, vy, vtheta, dt, delta_x, delta_y, delta_theta);
    
    // 5. 更新位姿
    robot_pose_.x += delta_x;
    robot_pose_.y += delta_y;
    robot_pose_.yaw += delta_theta;  // 更新偏航角
    
    // 5. 角度归一化
    // robot_pose_.z = normalizeAngle(robot_pose_.z);

    // 6. 详细调试信息
    // RCLCPP_INFO(get_logger(), 
    //     "\n====== Pose Update Debug ======\n"
    //     "Input Velocities:\n"
    //     "  vx: %.3f m/s\n"
    //     "  vy: %.3f m/s\n"
    //     "  vz: %.3f rad/s (%.1f deg/s)\n"
    //     "Time:\n"
    //     "  dt: %.3f s\n"
    //     "Position Change:\n"
    //     "  Δx: %.3f m\n"
    //     "  Δy: %.3f m\n"
    //     "  Δz: %.3f rad (%.1f deg)\n"
    //     "Current Pose:\n"
    //     "  x: %.3f m\n"
    //     "  y: %.3f m\n"
    //     "  z: %.3f rad (%.1f deg)\n"
    //     "===========================",
    //     vx, vy, 
    //     vtheta, vtheta * 180.0 / M_PI,
    //     dt,
    //     delta_x, delta_y, 
    //     delta_theta, delta_theta * 180.0 / M_PI,
    //     robot_pose_.x, robot_pose_.y, 
    //     robot_pose_.yaw, robot_pose_.yaw * 180.0 / M_PI
    // );
     // 6. 角度归一化
    robot_pose_.yaw = normalizeAngle(robot_pose_.yaw);
    
    // // 7. 保存当前速度用于下一次计算
    // robot_pose_.last_vx = vx;
    // robot_pose_.last_vy = vy;
    // robot_pose_.last_vtheta = vtheta;
    // 7. 更新上次更新时间
    last_update_time_ = current_time;
}

// 发布机器人状态
void ChassisDriver::publishRobotState()
{
    // static int publish_count = 0;
    // const int PUBLISH_DIVIDER = 2;
    
    // if (++publish_count >= PUBLISH_DIVIDER) 
    {
        // publish_count = 0;
        publishOdometry();
        publishVoltage();
    }
}

// 发布里程计信息
void ChassisDriver::publishOdometry()
{
    // 简单的直行修正：如果角速度很小，强制vy为0
    bool is_straight_motion = std::abs(robot_velocity_.angular_z) < 0.05; // 角速度小于0.05 rad/s
    bool is_moving = std::abs(robot_velocity_.linear_x) > 0.01; // 线速度大于0.01 m/s
    
    if (is_straight_motion && is_moving) {
        robot_velocity_.linear_y = 0.0; // 强制横向速度为0
        RCLCPP_DEBUG(get_logger(), "Applied straight motion correction");
    }
    
    // 创建并规范化四元数
    tf2::Quaternion q;
    q.setRPY(0, 0, robot_pose_.yaw);
    q.normalize();  // 确保四元数被规范化
    geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q);
    
    // 创建里程计消息
    nav_msgs::msg::Odometry odom;
    odom.header.stamp = rclcpp::Node::now();
    odom.header.frame_id = odom_frame_id_;
    odom.child_frame_id = robot_frame_id_;
    
    // 设置位置
    odom.pose.pose.position.x = robot_pose_.x;
    odom.pose.pose.position.y = robot_pose_.y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;
    
    // 设置速度
    odom.twist.twist.linear.x = robot_velocity_.linear_x;
    odom.twist.twist.linear.y = robot_velocity_.linear_y; // 可能已被修正为0
    odom.twist.twist.angular.z = robot_velocity_.angular_z;
    
    // 设置协方差矩阵 - 针对直行运动调整不确定性
    for (int i = 0; i < 36; ++i) {
        odom.pose.covariance[i] = 0.0;
        odom.twist.covariance[i] = 0.0;
    }

    // 位置协方差
    odom.pose.covariance[0] = 0.001;  // x位置
    odom.pose.covariance[7] = is_straight_motion ? 0.0001 : 0.001;  // y位置 - 直行时更确定
    odom.pose.covariance[35] = 0.01;  // yaw角度

    // 速度协方差
    odom.twist.covariance[0] = 0.001;  // vx
    odom.twist.covariance[7] = is_straight_motion ? 0.0001 : 0.001;  // vy - 直行时更确定
    odom.twist.covariance[35] = 0.01;  // vyaw
    
    // 发布里程计消息
    odom_publisher_->publish(odom);
}

// 发布电压信息
void ChassisDriver::publishVoltage()
{
    std_msgs::msg::Float32 voltage_msg;
    voltage_msg.data = battery_voltage_;
    voltage_publisher_->publish(voltage_msg);
}

// 速度命令回调
void ChassisDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    if (!socket_initialized_) return;

    // 1. 设置帧头和预留位
    // send_buffer_[0] = FRAME_HEADER;
    // send_buffer_[1] = 0;

    // 2. 处理线速度和角速度 (放大10000倍转换为整数)
    int16_t vx_i = static_cast<int16_t>(msg->linear.x * 1000.0f);
    int16_t vy_i = static_cast<int16_t>(msg->linear.y * 1000.0f);
    int16_t vw_i = static_cast<int16_t>(msg->angular.z * 1000.0f);
    
    // // 2. 放大并四舍五入
    // int16_t vx_i = (int16_t)round(msg->linear.x * 1000.0f);
    // int16_t vy_i = (int16_t)round(msg->linear.y * 1000.0f);
    // int16_t vw_i = (int16_t)round(msg->angular.z * 1000.0f);

    // 3. 填充速度数据
    send_buffer_[2] = (vx_i >> 8) & 0xFF;
    send_buffer_[3] = vx_i & 0xFF;
    send_buffer_[4] = (vy_i >> 8) & 0xFF;
    send_buffer_[5] = vy_i & 0xFF;
    send_buffer_[6] = (vw_i >> 8) & 0xFF;
    send_buffer_[7] = vw_i & 0xFF;

    // 4. 计算校验和
    send_buffer_[8] = calculateChecksum(send_buffer_, 8);

    // 5. 设置帧尾
    // send_buffer_[9] = FRAME_TAIL;

    // 6. 发送数据
    if (!sendData(send_buffer_, SEND_DATA_SIZE)) {
        RCLCPP_ERROR(get_logger(), "Failed to send velocity command");
    }
}

// 读取传感器数据
bool ChassisDriver::waitData()
{
    if (!socket_initialized_) return false;
    
    if (!receiveData(receive_buffer_, RECEIVE_DATA_SIZE)) {
        return false;
    }

    // 1. 检查帧头帧尾
    if (receive_buffer_[0] != FRAME_HEADER || receive_buffer_[9] != FRAME_TAIL) {
        RCLCPP_WARN(get_logger(), "Invalid frame header or tail");
        return false;
    }

    // 2. 校验和检查
    if (calculateChecksum(receive_buffer_, 8) != receive_buffer_[8]) {
        RCLCPP_WARN(get_logger(), "Checksum error");
        return false;
    }

    // 3. 解析速度数据
    int16_t vx_raw = (receive_buffer_[2] << 8) | receive_buffer_[3];
    int16_t vy_raw = (receive_buffer_[4] << 8) | receive_buffer_[5];
    int16_t vw_raw = (receive_buffer_[6] << 8) | receive_buffer_[7];

    robot_velocity_.linear_x = static_cast<float>(vx_raw) / 1000.0f;
    robot_velocity_.linear_y = static_cast<float>(vy_raw) / 1000.0f;
    robot_velocity_.angular_z = static_cast<float>(vw_raw) / 1000.0f;
    static int count = 0;
    if (count % 50 == 0) {
        RCLCPP_DEBUG(get_logger(), "Received velocity: %f, %f, %f", 
                    robot_velocity_.linear_x, robot_velocity_.linear_y, robot_velocity_.angular_z);
    }
    count++;

    return true;
}

// Socket初始化
bool ChassisDriver::initSocket()
{
#ifdef _WIN32
    WSADATA wsaData;
    if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
        RCLCPP_ERROR(get_logger(), "Failed to initialize WinSock");
        return false;
    }
#endif

    udp_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
    if (udp_socket_ == INVALID_SOCKET) {
        RCLCPP_ERROR(get_logger(), "Failed to create socket: %s", strerror(errno));
        return false;
    }

    // 设置接收超时时间（可选）
    struct timeval tv;
    tv.tv_sec = 10;  // 10秒超时
    tv.tv_usec = 0;
    if (setsockopt(udp_socket_, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv)) < 0) {
        RCLCPP_WARN(get_logger(), "Failed to set receive timeout");
    }

    // 设置本地地址
    memset(&local_addr_, 0, sizeof(local_addr_));
    local_addr_.sin_family = AF_INET;
    local_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
    local_addr_.sin_port = htons(local_port_);

    // 绑定本地地址
    if (bind(udp_socket_, (struct sockaddr*)&local_addr_, sizeof(local_addr_)) == SOCKET_ERROR) {
        RCLCPP_ERROR(get_logger(), "Failed to bind socket: %s", strerror(errno));
        return false;
    }

    // 设置远程地址
    memset(&remote_addr_, 0, sizeof(remote_addr_));
    remote_addr_.sin_family = AF_INET;
    remote_addr_.sin_port = htons(remote_port_);
    if (inet_pton(AF_INET, remote_ip_.c_str(), &remote_addr_.sin_addr) <= 0) {
        RCLCPP_ERROR(get_logger(), "Invalid remote IP address: %s", remote_ip_.c_str());
        return false;
    }

    RCLCPP_INFO(get_logger(), "UDP socket initialized - Local port: %d, Remote: %s:%d",
                local_port_, remote_ip_.c_str(), remote_port_);
    return true;
}

// 发送数据
bool ChassisDriver::sendData(const uint8_t* data, size_t len)
{
    RCLCPP_DEBUG(get_logger(), "Sending data to %s:%d", remote_ip_.c_str(), remote_port_);
    ssize_t sent = sendto(udp_socket_, (const char*)data, len, 0,
                         (struct sockaddr*)&remote_addr_, sizeof(remote_addr_));
    if (sent < 0) {
        RCLCPP_ERROR(get_logger(), "Failed to send data: %s", strerror(errno));
        return false;
    }
    return static_cast<size_t>(sent) == len;
}

// 接收数据
bool ChassisDriver::receiveData(uint8_t* data, size_t len)
{
    struct sockaddr_in sender_addr;
    socklen_t sender_addr_len = sizeof(sender_addr);
    
#ifdef _WIN32
    int received = recvfrom(udp_socket_, (char*)data, len, 0,
                           (struct sockaddr*)&sender_addr, &sender_addr_len);
#else
    // 使用阻塞式接收，简化代码
    ssize_t received = recvfrom(udp_socket_, data, len, 0,
                               (struct sockaddr*)&sender_addr, &sender_addr_len);
#endif

    if (received < 0) {
        RCLCPP_ERROR(get_logger(), "Failed to receive data: %s", strerror(errno));
        return false;
    }
    
    // 打印接收到的数据（十六进制格式）
    std::stringstream hex_stream;
    for (size_t i = 0; i < static_cast<size_t>(received); i++) {
        hex_stream << std::hex << std::setw(2) << std::setfill('0') 
                  << static_cast<int>(data[i]) << " ";
    }
    RCLCPP_DEBUG(get_logger(), "Received %zd bytes: %s", 
                 received, hex_stream.str().c_str());
    
    return true;
}

// 关闭Socket
void ChassisDriver::closeSocket()
{
    if (udp_socket_ != INVALID_SOCKET) {
        closesocket(udp_socket_);
    }
#ifdef _WIN32
    WSACleanup();
#endif
}

// 计算校验和
uint8_t ChassisDriver::calculateChecksum(const uint8_t* data, size_t len)
{
    uint8_t sum = 0;
    for (size_t i = 0; i < len; i++) {
        sum ^= data[i];
    }
    return sum;
}

// 角度归一化
double ChassisDriver::normalizeAngle(double angle)
{
    while (angle > M_PI) angle -= 2 * M_PI;
    while (angle < -M_PI) angle += 2 * M_PI;
    return angle;
}

double ChassisDriver::calculateMovingAverage(std::deque<double>& history, double new_value) {
    // 添加新值
    history.push_back(new_value);
    
    // 保持固定大小
    if (history.size() > FILTER_SIZE) {
        history.pop_front();
    }
    
    // 计算平均值
    double sum = 0.0;
    for (double value : history) {
        sum += value;
    }
    return sum / history.size();
}

void ChassisDriver::updateVelocityHistory(double vx, double vy, double vtheta) {
    vx_history_.push_back(vx);
    vy_history_.push_back(vy);
    vtheta_history_.push_back(vtheta);
    
    if (vx_history_.size() > FILTER_SIZE) {
        vx_history_.pop_front();
        vy_history_.pop_front();
        vtheta_history_.pop_front();
    }
}

void ChassisDriver::calculateKinematics(double vx, double vy, double vtheta, double dt,
                                      double& delta_x, double& delta_y, double& delta_theta) {
    // 使用梯形积分方法计算位置变化
    double avg_vx = (vx + robot_pose_.last_vx) * 0.5;
    double avg_vy = (vy + robot_pose_.last_vy) * 0.5;
    double avg_vtheta = (vtheta + robot_pose_.last_vtheta) * 0.5;
    
    // 考虑当前朝向的旋转矩阵
    double cos_yaw = cos(robot_pose_.yaw);
    double sin_yaw = sin(robot_pose_.yaw);
    
    // 计算在全局坐标系下的速度
    double global_vx = avg_vx * cos_yaw - avg_vy * sin_yaw;
    double global_vy = avg_vx * sin_yaw + avg_vy * cos_yaw;
    
    // 计算位置变化
    delta_x = global_vx * dt;
    delta_y = global_vy * dt;
    delta_theta = avg_vtheta * dt;
    
    // 添加二阶项以提高精度
    if (std::abs(avg_vtheta) > 1e-6) {
        double factor = 0.5 * avg_vtheta * dt;
        delta_x += avg_vy * factor;
        delta_y -= avg_vx * factor;
    }
}

// 主函数
int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ChassisDriver>();
    node->runLoop();
    rclcpp::shutdown();
    return 0;
}